#include <atomic>
#include <future>
#include <iostream>
#include <thread>
#include <cassert>
#include <string>

#include <mavsdk/mavsdk.h>
#include <mavsdk/plugins/mavlink_passthrough/mavlink_passthrough.h>
#include <mavsdk/plugins/telemetry/telemetry.h>
#include <mavsdk/plugins/action/action.h>
#include <mavsdk/plugins/param_server/param_server.h>
#include <mavsdk/plugins/param/param.h>
#include <mavsdk/plugins/telemetry_server/telemetry_server.h>
#include <mavsdk/plugins/action_server/action_server.h>

using namespace mavsdk;

using std::chrono::seconds;
using std::chrono::milliseconds;
using std::this_thread::sleep_for;


class MavsdkHandler
{
private:
  mavsdk::Mavsdk mavsdk{mavsdk::Mavsdk::Configuration{mavsdk::ComponentType::Autopilot}};
  std::string _drone_url{"udpin://0.0.0.0:14540"};
  std::string _ground_url{"udpout://127.0.0.1:14551"};

  bool drone_conn_state{false};
  bool ground_conn_state{false};

  int _systems_count{0};

  std::shared_ptr<System> _drone;
  std::shared_ptr<System> _ground;
  std::shared_ptr<ServerComponent> _server;

public:
  MavsdkHandler(){

    mavsdk.subscribe_on_new_system([this](){
      _systems_count = this->mavsdk.systems().size();
      
      if(_systems_count > 0 && drone_conn_state || _systems_count > 1)
      {
        _ground = mavsdk.systems()[1];
        _ground->subscribe_is_connected([this](bool conn_state){
          this->ground_conn_state = conn_state;
        });
      }
      else
        ground_conn_state = false;  
    });

    _connect_to_drone();
    _connect_to_ground();

    bool tmp_drone_state{false};
    bool tmp_ground_state{false};

    while(true){
      if(tmp_drone_state != drone_conn_state)
      {
        tmp_drone_state = drone_conn_state;
        std::cout << "DCS:\t" << drone_conn_state << "\n";
      }

      if(tmp_ground_state != ground_conn_state)
      {
        tmp_ground_state = ground_conn_state;
        std::cout << "GCS:\t" << ground_conn_state << "\n";
      }
    }
  }

  void _connect_to_drone()
  {
    auto result = mavsdk.add_any_connection_with_handle(_drone_url);
    assert(result.first == mavsdk::ConnectionResult::Success);

    _drone = mavsdk.first_autopilot(-1).value();
    _drone->subscribe_is_connected([this](bool conn_state){
      this->drone_conn_state = conn_state;
    });
  }

  void _connect_to_ground()
  {
    auto result = mavsdk.add_any_connection_with_handle(_ground_url);
    assert(result.first == mavsdk::ConnectionResult::Success);

    _server = mavsdk.server_component_by_type(ComponentType::GroundStation);
  }
};

int main(int argc, char** argv)
{
  MavsdkHandler mavsdk_handler;

  return 0;
/*
    auto system = mavsdk.first_autopilot(3.0);
    if (!system) {
        std::cerr << "Timed out waiting for system\n";
        return 1;
    }

    auto telemetry = Telemetry{system.value()};
    auto action = Action{system.value()};

    auto server_component = mavsdk.server_component();
    auto telemServer = mavsdk::TelemetryServer{server_component};

    const auto set_rate_result = telemetry.set_rate_battery(1.0);
    if (set_rate_result != Telemetry::Result::Success) {
        std::cerr << "Setting rate failed: " << set_rate_result << '\n';
        return 1;
    }
    
    telemetry.subscribe_battery([&telemServer](Telemetry::Battery battery) {
        TelemetryServer::Battery TS_battery;
        TS_battery.voltage_v = battery.voltage_v;
        TS_battery.remaining_percent = battery.remaining_percent / 100.0;

        std::cout << battery << "\n";
        std::cout << TS_battery << "\n";
        telemServer.publish_battery(TS_battery);
    });

    while(true);

    return 0;
*/
/*
    auto server_component = mavsdkTester.server_component();

    auto paramServer = mavsdk::ParamServer{server_component};
    auto telemServer = mavsdk::TelemetryServer{server_component};
    auto actionServer = mavsdk::ActionServer{server_component};

    paramServer.provide_param_int("MIS_TAKEOFF_ALT", 0);
    paramServer.provide_param_int("MY_PARAM", 1);

    actionServer.set_allowable_flight_modes({true, true, true});
    actionServer.set_allow_takeoff(true);
    actionServer.set_armable(true, true);

    TelemetryServer::Position position{55.953251, -3.188267, 0, 0};
    TelemetryServer::PositionVelocityNed positionVelocityNed{{0, 0, 0}, {0, 0, 0}};
    TelemetryServer::VelocityNed velocity{};
    TelemetryServer::Heading heading{60};
    TelemetryServer::RawGps rawGps{
        0, 55.953251, -3.188267, 0, NAN, NAN, 0, NAN, 0, 0, 0, 0, 0, 0};
    TelemetryServer::GpsInfo gpsInfo{11, TelemetryServer::FixType::Fix3D};
    TelemetryServer::Battery battery;

    telemServer.publish_home(position);

    actionServer.subscribe_takeoff([&position](ActionServer::Result result, bool takeoff) {
        if (result == ActionServer::Result::Success) {
            position.relative_altitude_m = 10;
        }
    });

    actionServer.subscribe_land([&position](ActionServer::Result result, bool land) {
        if (result == ActionServer::Result::Success) {
            position.relative_altitude_m = 0;
        }
    });

    while (true) {
        std::this_thread::sleep_for(std::chrono::seconds(1));

        telemServer.publish_home(position);
        telemServer.publish_sys_status(battery, true, true, true, true, true);
        telemServer.publish_position(position, velocity, heading);
        telemServer.publish_position_velocity_ned(positionVelocityNed);
        telemServer.publish_raw_gps(rawGps, gpsInfo);

        telemServer.publish_unix_epoch_time(42);
    }
*/
}
